//------------------------------------------------------------------------------
//  <copyright file="TestPokeFRIConnection.cpp" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//  </copyright>
//------------------------------------------------------------------------------

#include "stdafx.h"
#include "TestPokeFRIConnection.h"
#include "KukaLBR4NativeTests.h"

/// <summary> Initializes a new instance of the TestPokeFRIConnection class. </summary>
TestPokeFRIConnection::TestPokeFRIConnection() : MockFRIConnection()
{
	this->gotPositionCommand = false;
}

/// <summary> Destructor. </summary>
TestPokeFRIConnection::~TestPokeFRIConnection()
{
	// Fail test if we did not get a position command prior to destructor being called.
	KUKALBR4NATIVEASSERT(this->gotPositionCommand, "Position command received");
}

/// <summary> Close the FRI server. </summary>
/// <returns> Status code.  0 on no error. </returns>
int TestPokeFRIConnection::CloseFRI()
{
	// Fail test if we try to close the FRI connection without having received a poistion command
	KUKALBR4NATIVEASSERT(this->gotPositionCommand, "Position command received");

	return MockFRIConnection::CloseFRI();
}

/// <summary> Execute joint position control. </summary>
/// <param name="jointPosition"> Array of joint positions. </param>
/// <returns> Status code.  0 on no error. </returns>
int TestPokeFRIConnection::DoPositionControl(double *jointPosition)
{
	this->gotPositionCommand = true;

	for(int i = 0; i < NUM_JOINTS; i++)
	{
		// Expect the position control command to contain the position in <c>PokeJointAngleTarget</c>
		KUKALBR4NATIVEASSERT(jointPosition[i] == pPokeJointAngleTarget[i], "Position command received");
	}

	return MockFRIConnection::DoPositionControl(jointPosition);
}

/// <summary>
/// let the test code set the sensed force/torque at the TCP.
/// There is a potential race condition here (two threads can be accessing the feedback
/// register at the same time).  None of the current tests will be adversely effected by
/// this, but synchonization should be implemented if future tests require it.
/// </summary>
/// <param name="pTCPEstimatedTorque"> The values to set the estimted TCP torque register to. </param>
void TestPokeFRIConnection::SetTCPEstimatedTorque(double *pTCPEstimatedTorque)
{
	double *pTCPEstimatedTorqueRegister = this->pLbrFeedback->GetRegister(
		KukaLBR4PlusFeedbackRegisters::EstimatedTCPForceTorqueRegister);

	for(unsigned int i = 0; i < CARTESIAN_DIM; ++i)
	{
		pTCPEstimatedTorqueRegister[i] = pTCPEstimatedTorque[i];
	}
}
